﻿#include "opencv2/opencv.hpp"

using namespace cv;
using namespace std;

const int MAX_POINT_COUNT = 100;
const double MIN_DISTANCE = 7.0f;
const double QUALITY_LEVEL = 0.3;

//4. 使用光流估计方法，在前述测试视频上计算特征点，进一步进行特征点光流估计。

//发现特征点一旦经过中央柱子，就会被舍弃掉


int main()
{
	const char* fn = ".\\768x576.avi";
	VideoCapture cap;
	Mat  old_frame, old_gray, cur_frame, cur_gray, mask;
	vector<Point2f> points_pre;
	vector<Point2f> points_cur;
	vector<uchar> status; // 每一特征点检测状态
	vector<float> err; // 每一特征点计算误差
	vector<Scalar> line_color_cfgs; //画线用的颜色
	int frame_idx = 0;//第几帧

	RNG rng(time(0));

	//初始化画线用的颜色,先弄100个备用
	for (int i = 0; i < 100; i++)
	{
		line_color_cfgs.push_back(Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)));
	}
	
	cap.open(fn);
	if (!cap.isOpened())
		cout << "无法打开视频文件： " << fn << endl;

	//拿到第一帧图像
	cap >> old_frame;
	if (old_frame.empty())
	{
		return 0;
	}

	mask = Mat(Size(old_frame.cols, old_frame.rows), CV_8UC3, Scalar(0, 0, 0));

	cvtColor(old_frame, old_gray, COLOR_BGR2GRAY);

	//只拿第一帧的特征点做观察，随着时间推移逐渐被status舍弃掉，特征点全都被舍弃，会再次调用goodFeaturesToTrack
	goodFeaturesToTrack(old_gray, points_pre, MAX_POINT_COUNT, QUALITY_LEVEL, MIN_DISTANCE);

	for (;;)
	{
		cap >> cur_frame;
		if (cur_frame.empty())
		{
			break;
		}
		frame_idx++;
		cvtColor(cur_frame, cur_gray, COLOR_BGR2GRAY);

		//需要传入前一帧和当前图像以及前一帧检测到的角点
		calcOpticalFlowPyrLK(old_gray, cur_gray, points_pre, points_cur, status, err);
		// 下面删除掉误判点
		int counter = 0;
		for (int i = 0; i < points_cur.size(); i++)
		{
			double dist = norm(points_cur[i] - points_pre[i]);
			if (status[i] && dist >= 2.0 && dist <= 20.0) // 合理的特征追踪点
			{
				points_pre[counter] = points_pre[i];
				points_cur[counter] = points_cur[i];
				counter++;
			}
		}
		points_pre.resize(counter);
		points_cur.resize(counter);
		cout << "第" << frame_idx << "帧  " << "  counter:" << counter << endl;
		if (counter <= 0)
		{
			goodFeaturesToTrack(cur_gray, points_pre, MAX_POINT_COUNT, QUALITY_LEVEL, MIN_DISTANCE);//点特征点了，再用一次
			continue;
		}

		//绘制踪迹
		for (int i = 0; i < counter; i++)
		{
			line(mask, points_pre[i], points_cur[i], line_color_cfgs[i], 2);
			circle(cur_frame, points_cur[i], 5, line_color_cfgs[i],5);
		}
		Mat show_img;
		add(cur_frame, mask, show_img);

		//预备下一帧数据
		swap(points_pre, points_cur);
		swap(old_gray, cur_gray);

		imshow("show_img", show_img);
		// 以下检测是否终止(按下ESC终止，对应ASCII 27)
		char key = waitKey(150);
		if (key == 27)
			break;
	}
}
